#include "obj-sensor-sample-v7.h"

namespace obj_sensor_sample_v7
{
	class ObjectSensorSampleVars : public SessionSampleVars
	{
	public:
		SPADAS_VARS(ObjectSensorSample, SessionSampleVars)
		ObjectSensorSampleData data;

		SessionGeneralSample toGeneralSample()
		{
			SessionGeneralSample sample;
			sample.timestamp() = timestamp;

			UInt trajectoryPoints = 0;
			UInt contourPoints = 0;
			Array<UInt> trajectoryOffset(data.objects.size());
			Array<UInt> trajectorySize(data.objects.size());
			Array<UInt> contourOffset(data.objects.size());
			Array<UInt> contourSize(data.objects.size());
			for (UInt i = 0; i < data.objects.size(); i++)
			{
				trajectoryOffset[i] = trajectoryPoints;
				trajectorySize[i] = data.objects[i].trajectory.size();
				trajectoryPoints += trajectorySize[i];
				contourOffset[i] = contourPoints;
				contourSize[i] = data.objects[i].contour.size();
				contourPoints += contourSize[i];
			}

			Array<GeneralElement> values(20 + data.objects.size() * 72 + data.fovs.size() * 6 + (trajectoryPoints + contourPoints) * 2);
			sample.values() = values;

			UInt objBase = 20;
			UInt fovBase = objBase + data.objects.size() * 72;
			UInt trajectoryBase = fovBase + data.fovs.size() * 6;
			UInt contourBase = trajectoryBase + trajectoryPoints * 2;

			values[0] = (Double)data.objects.size();
			values[1] = (Double)data.fovs.size();
			values[2] = (Double)trajectoryPoints;
			values[3] = (Double)contourPoints;

			values[4] = data.koFrontIndex.valid ? (Double)data.koFrontIndex.value : OptionalDouble();
			values[5] = data.koLeftIndex.valid ? (Double)data.koLeftIndex.value : OptionalDouble();
			values[6] = data.koRightIndex.valid ? (Double)data.koRightIndex.value : OptionalDouble();

			values[7] = data.vehiVX;
			values[8] = data.vehiCurv;
			values[9] = data.vehiWidth;
			values[10] = data.vehiLength;
			values[11] = data.vehiWheelBase;
			values[12] = data.vehiFrontOverhang;

			for (UInt i = 0; i < data.objects.size(); i++)
			{
				ObjectInfo& src = data.objects[i];
				UInt baseIndex = objBase + 72 * i;
				values[baseIndex + 0] = (Double)src.id;
				values[baseIndex + 1] = (Double)src.age;
				values[baseIndex + 2] = src.rawID.valid ? (Double)src.rawID.value : OptionalDouble();
				values[baseIndex + 3] = src.rawAge.valid ? (Double)src.rawAge.value : OptionalDouble();
				values[baseIndex + 4] = src.rawClassID.valid ? (Double)src.rawClassID.value : OptionalDouble();
				values[baseIndex + 5] = (UInt)src.classs;
				values[baseIndex + 6] = (UInt)src.posMode;
				values[baseIndex + 7] = src.posx;
				values[baseIndex + 8] = src.posy;
				values[baseIndex + 9] = src.posz;
				values[baseIndex + 10] = src.posxSigma;
				values[baseIndex + 11] = src.posySigma;
				values[baseIndex + 12] = src.poszSigma;
				values[baseIndex + 13] = src.cpx;
				values[baseIndex + 14] = src.cpy;
				values[baseIndex + 15] = src.cpd;
				values[baseIndex + 16] = src.width;
				values[baseIndex + 17] = src.length;
				values[baseIndex + 18] = src.height;
				values[baseIndex + 19] = src.heading;
				values[baseIndex + 20] = src.vxRel;
				values[baseIndex + 21] = src.vxAbs;
				values[baseIndex + 22] = src.vyRel;
				values[baseIndex + 23] = src.vyAbs;
				values[baseIndex + 24] = src.vzRel;
				values[baseIndex + 25] = src.vzAbs;
				values[baseIndex + 26] = src.axRel;
				values[baseIndex + 27] = src.axAbs;
				values[baseIndex + 28] = src.ayRel;
				values[baseIndex + 29] = src.ayAbs;
				values[baseIndex + 30] = src.azRel;
				values[baseIndex + 31] = src.azAbs;
				values[baseIndex + 32] = src.color.valid ? (Double)src.color.r : OptionalDouble();
				values[baseIndex + 33] = src.color.valid ? (Double)src.color.g : OptionalDouble();
				values[baseIndex + 34] = src.color.valid ? (Double)src.color.b : OptionalDouble();
				values[baseIndex + 35] = src.timeOffset.valid ? (Double)src.timeOffset.value : OptionalDouble();
				values[baseIndex + 36] = src.confidence;
				values[baseIndex + 37] = src.classConfidence;
				values[baseIndex + 38] = data.getTTC1(src);
				values[baseIndex + 39] = data.getTTC2(src);
				values[baseIndex + 40] = data.getTHW(src);
				values[baseIndex + 41] = data.getDCA(src, 1.0);
				values[baseIndex + 42] = data.getDCA(src, 0.0);
				values[baseIndex + 43] = data.getOverlap(src);
				values[baseIndex + 44] = data.getLC(src);
				values[baseIndex + 45] = data.getRC(src);
				values[baseIndex + 46] = src.rcs;
				values[baseIndex + 47] = src.vLng;
				values[baseIndex + 48] = src.vLat;
				values[baseIndex + 49] = src.yawRate;
				values[baseIndex + 50] = src.curvature;
				values[baseIndex + 51] = src.speed();
				values[baseIndex + 68] = (Double)trajectoryOffset[i];
				values[baseIndex + 69] = (Double)trajectorySize[i];
				values[baseIndex + 70] = (Double)contourOffset[i];
				values[baseIndex + 71] = (Double)contourSize[i];


				UInt dataOffset = trajectoryBase + trajectoryOffset[i] * 2;
				for (UInt n = 0; n < trajectorySize[i]; n++)
				{
					Point2D& pt = src.trajectory[n];
					values[dataOffset + 2 * n] = pt.x;
					values[dataOffset + 2 * n + 1] = pt.y;
				}

				dataOffset = contourBase + contourOffset[i] * 2;
				for (UInt n = 0; n < contourSize[i]; n++)
				{
					Point2D& pt = src.contour[n];
					values[dataOffset + 2 * n] = pt.x;
					values[dataOffset + 2 * n + 1] = pt.y;
				}
			}

			for (UInt i = 0; i < data.fovs.size(); i++)
			{
				ObjectSensorFov& src = data.fovs[i];
				UInt baseIndex = fovBase + 6 * i;
				values[baseIndex + 0] = src.positionX;
				values[baseIndex + 1] = src.positionY;
				values[baseIndex + 2] = src.orientation;
				values[baseIndex + 3] = src.angleRange;
				values[baseIndex + 4] = src.distanceRange;
				values[baseIndex + 5] = src.blindRange;
			}

			sample.significantCount() = trajectoryBase;
			return sample;
		}

		Bool fromSample(String protocol, SessionSample sample) override
		{
			if (!sample.is<SessionGeneralSample>()) return FALSE;

			auto generalSample = sample.as<SessionGeneralSample>();
			timestamp = generalSample.timestamp();
			auto values = generalSample.values();

			if (protocol == "obj-sensor-sample-v7")
			{
				if (values.size() < 20) return FALSE;

				if (!values[0].valid ||
					!values[1].valid ||
					!values[2].valid ||
					!values[3].valid) return FALSE;

				Int nObjects = (Int)values[0].value;
				Int nFovs = (Int)values[1].value;
				Int nTrajectories = (Int)values[2].value;
				Int nContours = (Int)values[3].value;

				Int sizeWithExtra = 20 + nObjects * 72 + nFovs * 6 + (nTrajectories + nContours) * 2;
				Int sizeWithoutExtra = 20 + nObjects * 72 + nFovs * 6;
				if ((Int)values.size() != sizeWithExtra && (Int)values.size() != sizeWithoutExtra) return false;

				data.koFrontIndex = values[4].valid ? (Int)values[4].value : OptionalInt();
				data.koLeftIndex = values[5].valid ? (Int)values[5].value : OptionalInt();
				data.koRightIndex = values[6].valid ? (Int)values[6].value : OptionalInt();

				data.vehiVX = values[7];
				data.vehiCurv = values[8];
				data.vehiWidth = values[9];
				data.vehiLength = values[10];
				data.vehiWheelBase = values[11];
				data.vehiFrontOverhang = values[12];

				Int objBase = 20;
				Int fovBase = objBase + 72 * nObjects;
				Int trajBase = fovBase + 6 * nFovs;
				Int contBase = trajBase + 2 * nTrajectories;

				data.objects = Array<ObjectInfo>(nObjects);
				for (Int i = 0; i < nObjects; i++)
				{
					ObjectInfo& obj = data.objects[i];
					UInt baseIndex = objBase + 72 * i;

					obj.id = (Int)values[baseIndex + 0].value;
					obj.age = (Int)values[baseIndex + 1].value;
					obj.rawID = values[baseIndex + 2].valid ? (Int)values[baseIndex + 2].value : OptionalInt();
					obj.rawAge = values[baseIndex + 3].valid ? (Int)values[baseIndex + 3].value : OptionalInt();
					obj.rawClassID = values[baseIndex + 4].valid ? (Int)values[baseIndex + 4].value : OptionalInt();
					obj.classs = (ObjectClass)(Int)values[baseIndex + 5].value;
					obj.posMode = (PositionMode)(Int)values[baseIndex + 6].value;
					obj.posx = values[baseIndex + 7].value;
					obj.posy = values[baseIndex + 8].value;
					obj.posz = values[baseIndex + 9].value;
					obj.posxSigma = values[baseIndex + 10];
					obj.posySigma = values[baseIndex + 11];
					obj.poszSigma = values[baseIndex + 12];
					obj.cpx = values[baseIndex + 13].value;
					obj.cpy = values[baseIndex + 14].value;
					obj.cpd = values[baseIndex + 15].value;
					obj.width = values[baseIndex + 16];
					obj.length = values[baseIndex + 17];
					obj.height = values[baseIndex + 18];
					obj.heading = values[baseIndex + 19];
					obj.vxRel = values[baseIndex + 20];
					obj.vxAbs = values[baseIndex + 21];
					obj.vyRel = values[baseIndex + 22];
					obj.vyAbs = values[baseIndex + 23];
					obj.vzRel = values[baseIndex + 24];
					obj.vzAbs = values[baseIndex + 25];
					obj.axRel = values[baseIndex + 26];
					obj.axAbs = values[baseIndex + 27];
					obj.ayRel = values[baseIndex + 28];
					obj.ayAbs = values[baseIndex + 29];
					obj.azRel = values[baseIndex + 30];
					obj.azAbs = values[baseIndex + 31];
					obj.color.r = values[baseIndex + 32].valid ? (Byte)values[baseIndex + 32].value : 0;
					obj.color.g = values[baseIndex + 33].valid ? (Byte)values[baseIndex + 33].value : 0;
					obj.color.b = values[baseIndex + 34].valid ? (Byte)values[baseIndex + 34].value : 0;
					obj.color.valid = values[baseIndex + 32].valid && values[baseIndex + 33].valid && values[baseIndex + 34].valid;
					obj.timeOffset = values[baseIndex + 35].valid ? (Int)values[baseIndex + 35].value : OptionalInt();
					obj.confidence = values[baseIndex + 36];
					obj.classConfidence = values[baseIndex + 37];
					obj.rcs = values[baseIndex + 46];
					obj.vLng = values[baseIndex + 47];
					obj.vLat = values[baseIndex + 48];
					obj.yawRate = values[baseIndex + 49];
					obj.curvature = values[baseIndex + 50];

					Int trajOffset = (Int)values[baseIndex + 68].value;
					Int trajSize = (Int)values[baseIndex + 69].value;
					Int contOffset = (Int)values[baseIndex + 70].value;
					Int contSize = (Int)values[baseIndex + 71].value;

					if ((Int)values.size() == sizeWithExtra)
					{
						obj.trajectory = Array<Point2D>(trajSize);
						for (int n = 0; n < trajSize; n++)
						{
							obj.trajectory[n].x = (Float)values[trajBase + 2 * trajOffset + 2 * n].value;
							obj.trajectory[n].y = (Float)values[trajBase + 2 * trajOffset + 2 * n + 1].value;
						}

						obj.contour = Array<Point2D>(contSize);
						for (int n = 0; n < contSize; n++)
						{
							obj.contour[n].x = (Float)values[contBase + 2 * contOffset + 2 * n].value;
							obj.contour[n].y = (Float)values[contBase + 2 * contOffset + 2 * n + 1].value;
						}
					}
					else
					{
						obj.trajectory = Array<Point2D>();
						obj.contour = Array<Point2D>();
					}
				}

				data.fovs = Array<ObjectSensorFov>(nFovs);
				for (int i = 0; i < nFovs; i++)
				{
					ObjectSensorFov& fov = data.fovs[i];
					Int baseIndex = fovBase + 6 * i;

					fov.positionX = values[baseIndex].value;
					fov.positionY = values[baseIndex + 1].value;
					fov.orientation = values[baseIndex + 2].value;
					fov.angleRange = values[baseIndex + 3].value;
					fov.distanceRange = values[baseIndex + 4].value;
					fov.blindRange = values[baseIndex + 5].value;
				}

				return TRUE;
			}
			else if (protocol == "obj-sensor-sample-v6")
			{
				if (values.size() < 16) return FALSE;

				if (!values[0].valid ||
					!values[1].valid ||
					!values[2].valid ||
					!values[3].valid) return FALSE;

				Int nObjects = (Int)values[0].value;
				Int nFovs = (Int)values[1].value;
				Int nTrajectories = (Int)values[2].value;
				Int nContours = (Int)values[3].value;

				Int sizeWithExtra = 16 + nObjects * 52 + nFovs * 6 + (nTrajectories + nContours) * 2;
				Int sizeWithoutExtra = 16 + nObjects * 52 + nFovs * 6;
				if ((Int)values.size() != sizeWithExtra && (Int)values.size() != sizeWithoutExtra) return false;

				data.koFrontIndex = values[4].valid ? (Int)values[4].value : OptionalInt();
				data.koLeftIndex = values[5].valid ? (Int)values[5].value : OptionalInt();
				data.koRightIndex = values[6].valid ? (Int)values[6].value : OptionalInt();

				data.vehiVX = values[7];
				data.vehiCurv = values[8];
				data.vehiWidth = values[9];
				data.vehiLength = values[10];
				data.vehiWheelBase = values[11];
				data.vehiFrontOverhang = values[12];

				Int objBase = 16;
				Int fovBase = objBase + 52 * nObjects;
				Int trajBase = fovBase + 6 * nFovs;
				Int contBase = trajBase + 2 * nTrajectories;

				data.objects = Array<ObjectInfo>(nObjects);
				for (Int i = 0; i < nObjects; i++)
				{
					ObjectInfo& obj = data.objects[i];
					UInt baseIndex = objBase + 52 * i;

					obj.id = (Int)values[baseIndex + 0].value;
					obj.age = (Int)values[baseIndex + 1].value;
					obj.rawID = values[baseIndex + 2].valid ? (Int)values[baseIndex + 2].value : OptionalInt();
					obj.rawAge = values[baseIndex + 3].valid ? (Int)values[baseIndex + 3].value : OptionalInt();
					obj.rawClassID = values[baseIndex + 4].valid ? (Int)values[baseIndex + 4].value : OptionalInt();
					obj.classs = (ObjectClass)(Int)values[baseIndex + 5].value;
					obj.posMode = (PositionMode)(Int)values[baseIndex + 6].value;
					obj.posx = values[baseIndex + 7].value;
					obj.posy = values[baseIndex + 8].value;
					obj.cpx = values[baseIndex + 9].value;
					obj.cpy = values[baseIndex + 10].value;
					obj.cpd = values[baseIndex + 11].value;
					obj.width = values[baseIndex + 12];
					obj.length = values[baseIndex + 13];
					obj.heading = values[baseIndex + 14];
					obj.vxRel = values[baseIndex + 15];
					obj.vxAbs = values[baseIndex + 16];
					obj.vyRel = values[baseIndex + 17];
					obj.vyAbs = values[baseIndex + 18];
					obj.axRel = values[baseIndex + 19];
					obj.axAbs = values[baseIndex + 20];
					obj.ayRel = values[baseIndex + 21];
					obj.ayAbs = values[baseIndex + 22];
					obj.color.r = values[baseIndex + 23].valid ? (Byte)values[baseIndex + 23].value : 0;
					obj.color.g = values[baseIndex + 24].valid ? (Byte)values[baseIndex + 24].value : 0;
					obj.color.b = values[baseIndex + 25].valid ? (Byte)values[baseIndex + 25].value : 0;
					obj.color.valid = values[baseIndex + 23].valid && values[baseIndex + 24].valid && values[baseIndex + 25].valid;
					obj.timeOffset = values[baseIndex + 26].valid ? (Int)values[baseIndex + 26].value : OptionalInt();
					obj.confidence = values[baseIndex + 27];
					obj.classConfidence = values[baseIndex + 28];
					obj.height = values[baseIndex + 29];
					obj.posz = values[baseIndex + 30].value;
					obj.posxSigma = values[baseIndex + 31];
					obj.posySigma = values[baseIndex + 32];
					obj.poszSigma = values[baseIndex + 33];
					obj.rcs = values[baseIndex + 42];
					obj.vLng = values[baseIndex + 43];
					obj.vLat = values[baseIndex + 44];
					obj.yawRate = values[baseIndex + 45];
					obj.curvature = values[baseIndex + 46];

					Int trajOffset = (Int)values[baseIndex + 48].value;
					Int trajSize = (Int)values[baseIndex + 49].value;
					Int contOffset = (Int)values[baseIndex + 50].value;
					Int contSize = (Int)values[baseIndex + 51].value;

					if ((Int)values.size() == sizeWithExtra)
					{
						obj.trajectory = Array<Point2D>(trajSize);
						for (int n = 0; n < trajSize; n++)
						{
							obj.trajectory[n].x = (Float)values[trajBase + 2 * trajOffset + 2 * n].value;
							obj.trajectory[n].y = (Float)values[trajBase + 2 * trajOffset + 2 * n + 1].value;
						}

						obj.contour = Array<Point2D>(contSize);
						for (int n = 0; n < contSize; n++)
						{
							obj.contour[n].x = (Float)values[contBase + 2 * contOffset + 2 * n].value;
							obj.contour[n].y = (Float)values[contBase + 2 * contOffset + 2 * n + 1].value;
						}
					}
					else
					{
						obj.trajectory = Array<Point2D>();
						obj.contour = Array<Point2D>();
					}
				}

				data.fovs = Array<ObjectSensorFov>(nFovs);
				for (int i = 0; i < nFovs; i++)
				{
					ObjectSensorFov& fov = data.fovs[i];
					Int baseIndex = fovBase + 6 * i;

					fov.positionX = values[baseIndex].value;
					fov.positionY = values[baseIndex + 1].value;
					fov.orientation = values[baseIndex + 2].value;
					fov.angleRange = values[baseIndex + 3].value;
					fov.distanceRange = values[baseIndex + 4].value;
					fov.blindRange = values[baseIndex + 5].value;
				}

				return TRUE;
			}
			else if (protocol == "obj-sensor-sample-v5")
			{
				if (values.size() < 11) return FALSE;

				if (!values[0].valid ||
					!values[1].valid ||
					!values[2].valid ||
					!values[10].valid) return FALSE;

				Int nObjects = (Int)values[0].value;
				Int nTrajectories = (Int)values[1].value;
				Int nContours = (Int)values[2].value;
				Int nFovs = (Int)values[10].value;

				Int sizeWithExtra = 11 + nObjects * 42 + nFovs * 6 + (nTrajectories + nContours) * 2;
				Int sizeWithoutExtra = 11 + nObjects * 42 + nFovs * 6;
				if ((Int)values.size() != sizeWithExtra && (Int)values.size() != sizeWithoutExtra) return false;

				data.koFrontIndex = values[3].valid ? (Int)values[3].value : OptionalInt();
				data.koLeftIndex = values[4].valid ? (Int)values[4].value : OptionalInt();
				data.koRightIndex = values[5].valid ? (Int)values[5].value : OptionalInt();

				data.vehiVX = values[6];
				data.vehiCurv = values[7];
				data.vehiWidth = values[8];
				data.vehiLength = values[9];

				Int objBase = 11;
				Int fovBase = objBase + 42 * nObjects;
				Int trajBase = fovBase + 6 * nFovs;
				Int contBase = trajBase + 2 * nTrajectories;

				data.objects = Array<ObjectInfo>(nObjects);
				for (Int i = 0; i < nObjects; i++)
				{
					ObjectInfo& obj = data.objects[i];
					UInt baseIndex = objBase + 42 * i;

					obj.id = (Int)values[baseIndex + 0].value;
					obj.age = (Int)values[baseIndex + 1].value;
					obj.rawID = values[baseIndex + 2].valid ? (Int)values[baseIndex + 2].value : OptionalInt();
					obj.rawAge = values[baseIndex + 3].valid ? (Int)values[baseIndex + 3].value : OptionalInt();
					obj.rawClassID = values[baseIndex + 4].valid ? (Int)values[baseIndex + 4].value : OptionalInt();
					obj.classs = (ObjectClass)(Int)values[baseIndex + 5].value;
					obj.posMode = (PositionMode)(Int)values[baseIndex + 6].value;
					obj.posx = values[baseIndex + 7].value;
					obj.posy = values[baseIndex + 8].value;
					obj.cpx = values[baseIndex + 9].value;
					obj.cpy = values[baseIndex + 10].value;
					obj.cpd = values[baseIndex + 11].value;
					obj.width = values[baseIndex + 12];
					obj.length = values[baseIndex + 13];
					obj.heading = values[baseIndex + 14];
					obj.vxRel = values[baseIndex + 15];
					obj.vxAbs = values[baseIndex + 16];
					obj.vyRel = values[baseIndex + 17];
					obj.vyAbs = values[baseIndex + 18];
					obj.axRel = values[baseIndex + 19];
					obj.axAbs = values[baseIndex + 20];
					obj.ayRel = values[baseIndex + 21];
					obj.ayAbs = values[baseIndex + 22];
					obj.timeOffset = values[baseIndex + 35].valid ? (Int)values[baseIndex + 35].value : OptionalInt();
					obj.color.r = values[baseIndex + 36].valid ? (Byte)values[baseIndex + 36].value : 0;
					obj.color.g = values[baseIndex + 37].valid ? (Byte)values[baseIndex + 37].value : 0;
					obj.color.b = values[baseIndex + 38].valid ? (Byte)values[baseIndex + 38].value : 0;
					obj.color.valid = values[baseIndex + 36].valid && values[baseIndex + 37].valid && values[baseIndex + 38].valid;
					obj.classConfidence = values[baseIndex + 39];
					obj.posxSigma = values[baseIndex + 40];
					obj.posySigma = values[baseIndex + 41];

					Int trajOffset = (Int)values[baseIndex + 31].value;
					Int trajSize = (Int)values[baseIndex + 32].value;
					Int contOffset = (Int)values[baseIndex + 33].value;
					Int contSize = (Int)values[baseIndex + 34].value;

					if ((Int)values.size() == sizeWithExtra)
					{
						obj.trajectory = Array<Point2D>(trajSize);
						for (int n = 0; n < trajSize; n++)
						{
							obj.trajectory[n].x = (Float)values[trajBase + 2 * trajOffset + 2 * n].value;
							obj.trajectory[n].y = (Float)values[trajBase + 2 * trajOffset + 2 * n + 1].value;
						}

						obj.contour = Array<Point2D>(contSize);
						for (int n = 0; n < contSize; n++)
						{
							obj.contour[n].x = (Float)values[contBase + 2 * contOffset + 2 * n].value;
							obj.contour[n].y = (Float)values[contBase + 2 * contOffset + 2 * n + 1].value;
						}
					}
					else
					{
						obj.trajectory = Array<Point2D>();
						obj.contour = Array<Point2D>();
					}
				}

				data.fovs = Array<ObjectSensorFov>(nFovs);
				for (int i = 0; i < nFovs; i++)
				{
					ObjectSensorFov& fov = data.fovs[i];
					Int baseIndex = fovBase + 6 * i;

					fov.positionX = values[baseIndex].value;
					fov.positionY = values[baseIndex + 1].value;
					fov.orientation = values[baseIndex + 2].value;
					fov.angleRange = values[baseIndex + 3].value;
					fov.distanceRange = values[baseIndex + 4].value;
					fov.blindRange = values[baseIndex + 5].value;
				}

				return TRUE;
			}
			else return FALSE;
		}

		Bool supportInterpolation() override
		{
			return TRUE;
		}

		SessionSample interpolate(SessionSample& ss1, Double w1, SessionSample& ss2, Double w2, FullTimestamp timestamp) override
		{
			if (!ss1.is<ObjectSensorSample>() || !ss2.is<ObjectSensorSample>()) return SessionSample();

			ObjectSensorSampleData& s1 = ss1.as<ObjectSensorSample>().getVars()->data;
			ObjectSensorSampleData& s2 = ss2.as<ObjectSensorSample>().getVars()->data;
			
			// 创建输出对象
			ObjectSensorSample output;
			output.timestamp() = timestamp;
			ObjectSensorSampleData& so = output.getVars()->data;
			
			// 插值车辆基本参数
			so.vehiWidth = s1.vehiWidth;
			so.vehiLength = s1.vehiLength;
			so.vehiVX = s1.vehiVX * w1 + s2.vehiVX * w2;
			so.vehiCurv = s1.vehiCurv * w1 + s2.vehiCurv * w2;
			so.vehiWheelBase = s1.vehiWheelBase;
			so.vehiFrontOverhang = s1.vehiFrontOverhang;
			
			// 复制FOV信息
			so.fovs = s1.fovs;
			
			// 检查关键物体ID是否匹配
			OptionalInt koFrontID, koLeftID, koRightID;
			if (s1.koFrontIndex.valid && s2.koFrontIndex.valid && 
				s1.objects[s1.koFrontIndex.value].id == s2.objects[s2.koFrontIndex.value].id)
			{
				koFrontID = s1.objects[s1.koFrontIndex.value].id;
			}
			
			if (s1.koLeftIndex.valid && s2.koLeftIndex.valid && 
				s1.objects[s1.koLeftIndex.value].id == s2.objects[s2.koLeftIndex.value].id)
			{
				koLeftID = s1.objects[s1.koLeftIndex.value].id;
			}
			
			if (s1.koRightIndex.valid && s2.koRightIndex.valid && 
				s1.objects[s1.koRightIndex.value].id == s2.objects[s2.koRightIndex.value].id)
			{
				koRightID = s1.objects[s1.koRightIndex.value].id;
			}
			
			// 合并并插值目标物体 - 使用ArrayX动态添加
			ArrayX<ObjectInfo> objectsX;
			Int index = 0;
			
			for (auto o1 = s1.objects.firstElem(); o1.valid(); ++o1)
			{
				Int id = o1->id;
				for (auto o2 = s2.objects.firstElem(); o2.valid(); ++o2)
				{
					if (o2->id != id) continue;
					
					// 检查是否为关键物体并记录索引
					if (koFrontID.valid && koFrontID.value == id) so.koFrontIndex = index;
					if (koLeftID.valid && koLeftID.value == id) so.koLeftIndex = index;
					if (koRightID.valid && koRightID.value == id) so.koRightIndex = index;
					
					// 计算插值权重，考虑时间偏置
					Double ow1 = w1, ow2 = w2;
					if (o1->timeOffset.valid || o2->timeOffset.valid)
					{
						Double ot1 = ss1.as<ObjectSensorSample>().timestamp().offset;
						if (o1->timeOffset.valid) ot1 += 0.000001 * o1->timeOffset.value;
						Double ot2 = ss2.as<ObjectSensorSample>().timestamp().offset;
						if (o2->timeOffset.valid) ot2 += 0.000001 * o2->timeOffset.value;
						
						if (ot2 - ot1 >= 0.000001)
						{
							Double targetTime = ss1.as<ObjectSensorSample>().timestamp().offset + 
								(ss2.as<ObjectSensorSample>().timestamp().offset - ss1.as<ObjectSensorSample>().timestamp().offset) * w2;
							ow1 = (ot2 - targetTime) / (ot2 - ot1);
							ow2 = (targetTime - ot1) / (ot2 - ot1);
						}
					}
					
					// 创建插值目标物
					ObjectInfo obj;
					obj.id = id;
					obj.age = o1->age;
					obj.rawID = o1->rawID;
					obj.rawAge = o1->rawAge;
					obj.confidence = o1->confidence * ow1 + o2->confidence * ow2;
					obj.color = ow1 > ow2 ? o1->color : o2->color;
					obj.classs = ow1 > ow2 ? o1->classs : o2->classs;
					obj.rawClassID = ow1 > ow2 ? o1->rawClassID : o2->rawClassID;
					obj.classConfidence = o1->classConfidence * ow1 + o2->classConfidence * ow2;
					obj.posMode = o1->posMode;
					obj.posx = o1->posx * ow1 + o2->posx * ow2;
					obj.posy = o1->posy * ow1 + o2->posy * ow2;
					obj.posz = o1->posz * ow1 + o2->posz * ow2;
					obj.posxSigma = o1->posxSigma * ow1 + o2->posxSigma * ow2;
					obj.posySigma = o1->posySigma * ow1 + o2->posySigma * ow2;
					obj.poszSigma = o1->poszSigma * ow1 + o2->poszSigma * ow2;
					obj.cpx = o1->cpx * ow1 + o2->cpx * ow2;
					obj.cpy = o1->cpy * ow1 + o2->cpy * ow2;
					obj.cpd = o1->cpd * ow1 + o2->cpd * ow2;
					obj.vxAbs = o1->vxAbs * ow1 + o2->vxAbs * ow2;
					obj.vyAbs = o1->vyAbs * ow1 + o2->vyAbs * ow2;
					obj.vzAbs = o1->vzAbs * ow1 + o2->vzAbs * ow2;
					obj.axAbs = o1->axAbs * ow1 + o2->axAbs * ow2;
					obj.ayAbs = o1->ayAbs * ow1 + o2->ayAbs * ow2;
					obj.azAbs = o1->azAbs * ow1 + o2->azAbs * ow2;
					obj.vxRel = o1->vxRel * ow1 + o2->vxRel * ow2;
					obj.vyRel = o1->vyRel * ow1 + o2->vyRel * ow2;
					obj.vzRel = o1->vzRel * ow1 + o2->vzRel * ow2;
					obj.axRel = o1->axRel * ow1 + o2->axRel * ow2;
					obj.ayRel = o1->ayRel * ow1 + o2->ayRel * ow2;
					obj.azRel = o1->azRel * ow1 + o2->azRel * ow2;
					obj.rcs = o1->rcs * ow1 + o2->rcs * ow2;
					obj.vLng = o1->vLng * ow1 + o2->vLng * ow2;
					obj.vLat = o1->vLat * ow1 + o2->vLat * ow2;
					obj.yawRate = o1->yawRate * ow1 + o2->yawRate * ow2;
					obj.curvature = o1->curvature * ow1 + o2->curvature * ow2;
					
					// 角度插值处理
					if (o1->heading.valid && o2->heading.valid)
					{
						Double x1 = math::cos(o1->heading.value);
						Double y1 = math::sin(o1->heading.value);
						Double x2 = math::cos(o2->heading.value);
						Double y2 = math::sin(o2->heading.value);
						
						Double xo = x1 * ow1 + x2 * ow2;
						Double yo = y1 * ow1 + y2 * ow2;
						if (xo != 0 || yo != 0)
						{
							obj.heading = math::atan(yo, xo);
						}
						else
						{
							obj.heading = o1->heading;
						}
					}
					else
					{
						obj.heading = o1->heading;
					}
					
					obj.width = o1->width * ow1 + o2->width * ow2;
					obj.length = o1->length * ow1 + o2->length * ow2;
					obj.height = o1->height * ow1 + o2->height * ow2;
					
					// 对于轨迹点和轮廓点，取权重大的那个
					obj.trajectory = ow1 > ow2 ? o1->trajectory : o2->trajectory;
					obj.contour = ow1 > ow2 ? o1->contour : o2->contour;
					
					objectsX.append(obj);
					index++;
					break;
				}
			}
			
			// 转换为Array
			so.objects = objectsX.toArray();
			return output.as<SessionSample>();
		}
	};
}

using namespace obj_sensor_sample_v7;

ObjectSensorSample::ObjectSensorSample() : Object<ObjectSensorSampleVars>(new ObjectSensorSampleVars, TRUE)
{}

FullTimestamp& ObjectSensorSample::timestamp()
{
	return vars->timestamp;
}

ObjectSensorSampleData *ObjectSensorSample::operator ->()
{
    return &vars->data;
}

String ObjectSensorSample::getProtocolName(UInt channel)
{
    return (String)"obj-sensor-sample-v7@" + channel;
}

SessionGeneralSample ObjectSensorSample::toGeneralSample()
{
    return vars->toGeneralSample();
}

Array<String> ObjectSensorSample::getProtocolNames(UInt channel)
{
	return Array<String>::create(3, "obj-sensor-sample-v5", "obj-sensor-sample-v6", "obj-sensor-sample-v7").convert<String>([channel](auto& s){ return s + "@" + channel; });
}

Bool ObjectSensorSample::fromGeneralSample(String protocol, SessionGeneralSample generalSample)
{
    return vars->fromSample(protocol, generalSample.as<SessionSample>());
}